from alg import Alg
from robot_arm import RobotArm
from util_3d import *
from vision_3d import V3D
from camera_link_test import  Camera
from threading import *
import time


if __name__ == '__main__':
    alg = Alg()
    arm = RobotArm()
    ca = Camera()
    t1 = Thread(target=ca.update_image_loop)
    t1.start()

    base_t_end_grab_teach_dock_rokae_type = [0.08056891941297917, 0.5268137926645337, 0.14523333182881573,
                                             -1.5694936911265636, -0.0070184336014671835, 1.573066585680158]
    delta_T = xyzrpy_to_T(0.0,0.0,0.1,0.0,0.0,0.0)
    cur_T = arm.get_end_T()
    arm.movel_to_T(delta_T@cur_T)
    arm.wait_util()

    # base_t_end_grab_teach_dock_rokae_type[2] += 0.1
    # arm.movel_rokae(base_t_end_grab_teach_dock_rokae_type)
    # arm.wait_util()
    # base_t_end_grab_teach_dock_rokae_type[2] -= 0.1
    # arm.movel_rokae(base_t_end_grab_teach_dock_rokae_type)
    # arm.wait_util()

    arm.movel_to_T(alg.base_t_end_obs_teach)
    arm.wait_util()
    print("obs pose")
    time.sleep(3)

    # arm.movel_to_T(delta_T@alg.base_t_end_grab_teach)
    # arm.wait_util()
    # arm.movel_to_T(alg.base_t_end_grab_teach)
    # arm.wait_util()
    flag, cam_t_calib = ca.gen_cam_t_calib("T")
    if flag == True:
        base_t_end_grab_real = alg.gen_base_t_end_grab_real(cam_t_calib)
        arm.movel_to_T(delta_T@base_t_end_grab_real)
        arm.wait_util()
        arm.movel_to_T(base_t_end_grab_real)
        arm.wait_util()

    # alg.gen_base_t_end_grab_real(cam_obs_teach_t_calib_real)

    # arm.wait_util()
    # arm.movel_to_T(delta_T@alg.base_t_end_obs_teach)
    # print("obs pose z+0.1")
    # arm.wait_util()
    # arm.movel_to_T(delta_T@alg.base_t_end_grab_teach)
    # print("grab pose z+0.1")
    # arm.wait_util()
    # arm.movel_to_T(alg.base_t_end_grab_teach)
    # print("obs pose grab")
    # arm.wait_util()

